#ifndef TC_TOPOLOGY_CONTROLLER_HPP
#define TC_TOPOLOGY_CONTROLLER_HPP

#include "route_info.h" // route information API by Unghee and George
#include "tc_db_manager.hpp"
//#include <boost/graph/breadth_first_search.hpp>
#include <boost/graph/graphviz.hpp> // write_graphviz()
#include <iostream>
#include <vector>
#include <string>
#include <algorithm> // sort() and set_difference()
#include <queue> // for priority_queue
#include <pthread.h> // thread library
#include <signal.h> // signal name macros, and the signal() prototype


using namespace boost;
using std::string;

////////////
// COMPARE_FIRST struct
// Defines the compare function used by priority_queue

struct compare_first
{
  template <typename T>
  bool operator() (const T& x, const T& y)
  {
    return x.first > y.first;
  }
};


///////////
// PRINT_GRAPH()

void print_graph(Graph& G)
{
  graph_traits<Graph>::vertex_iterator vi, vi_end;
  graph_traits<Graph>::edge_iterator ed, ed_end;

  std::cout << std::endl << "vertices(G)" << std::endl;
  std::cout <<              "-----------" << std::endl;
  
  for (tie(vi, vi_end) = vertices(G); vi != vi_end; ++vi){
    std::cout << G[*vi].IP << " ; ";
    std::cout << G[*vi].MAC << " ; ";
    std::cout << "TPC: " << (G[*vi].TPC? "yes" : " no") << " ; ";
    std::cout << "Max_Power: " << G[*vi].MAX_POWER << " dBm" << " ; ";
    std::cout << std::endl;
  }
  std::cout << std::endl;

  std::cout << "  edges(G)  " << std::endl;
  std::cout << "------------" << std::endl;

  graph_traits<Graph>::vertex_descriptor u, v;

  for (tie(ed, ed_end) = edges(G); ed != ed_end; ++ed){
    u = source(*ed, G);
    v = target(*ed, G);
    std::cout << "Weight(" << G[u].IP << " , " << G[v].IP << ") = ";
    std::cout << G[*ed].Weight << std::endl;
  }
  std::cout << std::endl;
}

/////////////
// WRITE_GRAPH()

void write_graph(string file_name, Graph& G)
{
  typedef graph_traits<Graph>::vertex_descriptor Vertex;
  graph_traits<Graph>::vertex_descriptor u, v;
  graph_traits<Graph>::vertex_iterator vi, vi_end;
  graph_traits<Graph>::edge_iterator ed, ed_end;

  std::ofstream out(file_name.c_str());

  out << "Digraph G {" << std::endl;

  int i = 0;
  std::map<Vertex, int> v_index;
  for (tie(vi, vi_end) = vertices(G); vi != vi_end; ++vi, ++i){
    v_index[*vi] = i;
    out << i << "[label=\"" << G[*vi].IP << "\"];" << std::endl;
  }  

  for (tie(ed, ed_end) = edges(G); ed != ed_end; ++ed){
    u = source(*ed, G); v = target(*ed, G);
    out << v_index[u] << "->" << v_index[v]
        << " [label=\"" << G[*ed].Weight << "\"];" << std::endl;
  }

  out << "}" << std::endl;
}


/////////////
// EXIST_PATH()
// Non-member function to query the existance of a path from node u to node v in graph G

template <typename Vertex, typename Graph>
bool exist_path(const Vertex& source, 
		const Vertex& target, const Graph& G)
{
  //Typedefs
  typedef typename graph_traits<Graph>::vertex_iterator iter_t;
  typedef typename graph_traits<Graph>::adjacency_iterator adj_iter_t;

  Vertex u, v;
  u = source; v = target;
  iter_t ui, ui_end;
  adj_iter_t vi, vi_end;

  std::map<Vertex, default_color_type> color;
  std::queue<Vertex> Q;

  // for each vertex u in V[G]...
  for (tie(ui, ui_end) = vertices(G); ui != ui_end; ++ui){
    // color(u) = white
    color[*ui] = color_traits<default_color_type>::white();
  }
  
  // Start the search from node u
  color[u] = color_traits<default_color_type>::gray();
  Q.push(u); // discover vertex u

  while (!Q.empty()){
    u = Q.front(); // read the element at the front of the queue (BFS style)
    Q.pop(); // dequeue element
    for (tie(vi, vi_end) = adjacent_vertices(u, G); vi != vi_end; ++vi){
      // Check if target v has been found (there is a path from u to v)
      if (*vi == v){
	return true;
      }

      // If (u,v) is a tree edge, color(v) = gray and enqueue v
      if (color[*vi] == color_traits<default_color_type>::white()){
	color[*vi] = color_traits<default_color_type>::gray();
	Q.push(*vi);
      }
    }
    // Finish vertex u
    color[u] = color_traits<default_color_type>::black();
  }

  return false;
}

///////////////////////////////
// Thread prototype
void *TCVerifyTopology( void *ptr );



///////////////////////////////
// TOPOLOGY CONTROLLER class

class TCTopologyController{
public:
  // Default constructor
  TCTopologyController()
  {m_tx_power = NULL; m_tc_db = NULL; m_busy = false; m_open_loop = true;
   m_TC_T = 5; m_Idle_T = 3; m_Busy_T = 3; m_Tic = 0;}

  // Initialization constructor
  TCTopologyController(int& txpower, TCDatabaseManager& tc_db, bool open_loop,
                       double tc_t, double idle_t, double busy_t)
  {Init(txpower, tc_db, open_loop, tc_t, idle_t, busy_t);}

  // Initialization
  void Init(int& txpower, TCDatabaseManager& tc_db, bool open_loop,
            double tc_t, double idle_t, double busy_t)
  {m_tx_power = &txpower; m_tc_db = &tc_db; m_busy = false; m_open_loop = open_loop;
   m_TC_T = tc_t; m_Idle_T = idle_t; m_Busy_T = busy_t; m_Tic = 0;}

  // Set m_tx_power level, converting from [0,255] linear scale to driver scale
  void SetTxPower(int level) {*m_tx_power = ConvertToDriverScale(level);}

  // Get m_tx_power level, converting from driver scale to [0,255] linear scale
  int GetTxPower() {return ConvertFromDriverScale(*m_tx_power);}

  // Run the TC Algorithm and set m_TxPower accordingly
  void Run();

  // Resets the state of the Topology Controller (aborts all ongoing tasks)
  void Reset() {Unlock(); m_TC_Toc = m_Tic; m_Next_Toc = m_Tic;}

  // Updates the timer. On time-out of the topology controller,
  // the TC is unlocked and all timers are reset
  void UpdateTimer(double time)
  {m_Tic = time; if ((m_Tic - m_TC_Toc) > m_TC_T) Reset();}

  // TC Algorithm: calculates an output graph OutG on an input graph InG
  void TCAlg(Graph& InG, Graph& OutG);

private:
  typedef graph_traits<Graph>::vertex_descriptor vertex_t;

  double m_Tic;        // Final "time" for time-out calculations
  double m_TC_T;       // Time-out (in sec) to avoid infinite topology search
  double m_Idle_T;     // Time-out (in sec) for the next "idle state" event
  double m_Busy_T;     // Time-out (in sec) for the next "busy state" event
  double m_TC_Toc;     // Initial "time" when Run() is executed in idle state
  double m_Next_Toc;   // Initial "time" for the "next state event"

  bool m_busy;         // state control for multiple calls to Run()
  bool m_open_loop;    // open-loop strategy (true) / closed-loop (false)
                       // note: closed-loop => use of VerifyTopology()
  bool m_topol_match;  // flag for a match between the desired topology graph
                       // and the chosen Tx power level
  int m_change_power;  // flag set by VerifyTopology() and used by GraphToTxPower()
                       // -1: decrease ; 0: do nothing ; 1: increase
  TCDatabaseManager* m_tc_db; // Pointer to the Topology Database Manager
  Graph* m_graph;      // Pointer to the desired topology graph
                       // [used by GraphToTxPower() and VerifyTopology()]
  std::vector<string> m_nbr_list;  // list of target vertices (by IP address) in
                                   // the out_edges list of OutG
  std::vector<string> m_diff_list; // list of vertices (by IP address) present in
                                   // graph InG but not in graph OutG
  int* m_tx_power;     // Pointer to an external variable that controls
                       // the current power level (uses the driver scale)
  // The limits below are not in the driver's tx power level scale
  int m_tx_upper_lim;  // upper limit for selection of the m_tx_power
  int m_tx_lower_lim;  // lower limit for selection of the m_tx_power

  // Converts from [0,255] to the driver scale
  int ConvertToDriverScale(int linear_level);

  // Converts from the driver scale to [0,255]
  int ConvertFromDriverScale(int driver_level);

  // Lock access in Run()
  void Lock() {m_busy = true;}

  // Unlock access in Run()
  void Unlock() {m_busy = false;}

  // Sets the difference list (m_diff_list) and the neighbor list (m_nbr_list)
  bool CreateLists(Graph& InG, Graph& OutG);

  // Set m_change_power flag
  void SetChangePower(int value)
  {m_change_power = value;
   if (value == 0) m_topol_match = true; else m_topol_match = false;}

  // Sets m_TxPower to a value (power level) that is compatible with m_graph
  // (the root node should be the first node in the vertices list of m_graph)
  void GraphToTxPower();

  // Checks if the desired topology graph has been realized
  // Adjusts the m_tx_power level if necessary and unlocks the
  // Topology Controller when successful
  bool VerifyTopology();
};


////////
void TCTopologyController::Run()
{
  static Graph G, GG; // Input and output graphs respectively

  // Has the TC Controller been initialized?
  if (m_tx_power == NULL){
    std::cout << std::endl << "TCTopologyController: ";
    std::cout << "controller has not been initialized" << std::endl;
    return;
  }

  // Check if it is time to handle the next event
  double TIME_OUT;
  if (m_busy) TIME_OUT = m_Busy_T; else TIME_OUT = m_Idle_T;

  if ((m_Tic - m_Next_Toc) >= TIME_OUT){
    // Reset time to next event
    m_Next_Toc = m_Tic;
  }
  else{
    return; // if not, return
  }

  // Is the TC Controller already running?
  if (m_busy){
    // Has the desired topology been achieved?
    if (VerifyTopology() == true){
      Reset(); // reset the state of the Topology Controller
#if defined(DEBUG)
      std::cout << std::endl << "WE HAVE A TOPOLOGY MATCH!!!" << std::endl;
#endif
    }
    else { // not yet: adjust tx power
      GraphToTxPower();
    }
    return;
  } // endif m_busy
  

  // Is the topology graph ready?
  if (m_tc_db->GetGraph(G) == false){
    return;
  }

  // Run the TC algorithm to find the output graph
  TCAlg(G, GG);
  m_graph = &GG;

#if defined(DEBUG)
  // Write G and GG to disk in dot (Graphviz) format
  write_graph("input_graph.dot", G);
  system("dot -Tpng -oinput_graph.png input_graph.dot");
  write_graph("output_graph.dot", GG);
  system("dot -Tpng -ooutput_graph.png output_graph.dot");
#endif

  // Create the neighbor and diff lists
  if (CreateLists(G, GG) == false){
    Reset(); // the lists are not ready yet
    return;
  }

  // Check if the current topology is already suitable
  if (VerifyTopology() == true){
      Reset(); // reset the state of the Topology Controller
#if defined(DEBUG)
      std::cout << std::endl << "WE HAVE A TOPOLOGY MATCH!!!" << std::endl;
#endif
    return;
  }
  else { // If we do not have a match
    // Find a power level that is compatible with the output graph (m_graph)
    GraphToTxPower();
  }
}



////////
// Converts from [0,255] to the driver scale 
// driver scale: (max = 128 ; min = 127 ; middle = 0 or 255)
int TCTopologyController::ConvertToDriverScale(int linear_level)
{
  if (linear_level < 128){ // [0,127]
    return (127 - linear_level);
  }
  else { // [128,255]
    return (128 + (255 - linear_level));
  }
}

////////
// Converts from the driver scale to [0,255]
// driver scale: (max = 128 ; min = 127 ; middle = 0 or 255)
int TCTopologyController::ConvertFromDriverScale(int driver_level)
{
  // The logic is the same as ConvertToDriverScale()
  return ConvertToDriverScale(driver_level);
}


////////
bool TCTopologyController::CreateLists(Graph& InG, Graph& OutG)
{
  // Twos sets for the out_vertices (IP addresses) of InG and OutG
  std::vector<string> setA;
  std::vector<string>::iterator iter, iter_end;

  graph_traits<Graph>::vertex_descriptor u, v;
  graph_traits<Graph>::vertex_iterator ui, ui_end;
  graph_traits<Graph>::out_edge_iterator ed, ed_end;

  // Clear the neighbor list and the diff list
  m_nbr_list.clear();
  m_diff_list.clear();

  // The two graphs should have more than just the root node
  if ( (num_vertices(InG) < 2) || (num_vertices(OutG) < 2) ){
    return false;
  }

  // Populate setA (the source node u is the first node in InG)
  tie(ui, ui_end) = vertices(InG); u = *ui;
  for (tie(ed, ed_end) = out_edges(u, InG); ed != ed_end; ++ed){
    v = target(*ed, InG);
    setA.push_back( InG[v].IP );
  }

  // InG should have at least one out_edge
  if (setA.size() == 0){
    return false;
  }
  
  // Populate m_nbr_list (the source node u is the first node in OutG)
  tie(ui, ui_end) = vertices(OutG); u = *ui;
  for (tie(ed, ed_end) = out_edges(u, OutG); ed != ed_end; ++ed){
    v = target(*ed, OutG);
    m_nbr_list.push_back( OutG[v].IP );
  }

  // Resize the target list to the size of the larger set (i.e., A's)
  m_diff_list.resize( setA.size() );

  // Sort sets A and B (neighbor list), as required by set_difference
  sort( setA.begin(), setA.end() );
  sort( m_nbr_list.begin(), m_nbr_list.end() );

  // Perform the set difference among A and B (neighbor list)
  iter_end = set_difference( setA.begin(), setA.end(),
                             m_nbr_list.begin(), m_nbr_list.end(),
                             m_diff_list.begin() );

  // Resize and sort m_diff_list
  int new_size = 0;
  for (iter = m_diff_list.begin(); iter != iter_end; ++iter, ++new_size);
  m_diff_list.resize( new_size );
  sort( m_diff_list.begin(), m_diff_list.end() );

  return true;
}

////////
bool TCTopologyController::VerifyTopology()
{
  // Verify the topology using a separate thread
//  pthread_t tid; // thread identifier
//  pthread_create(&tid, NULL, TCVerifyTopology, (void*) this);

  int result = 0; // -1: decrease power ; 0: MATCH ; +1: increase power

  // MATCH: we have a match when the 1-hop neighbor list contains all nodes
  //        in m_nbr_list and does NOT contain any node in m_diff_list

  // Get IP address of the source node
  string ip, mac; bool tpc; int max_power;
  m_tc_db->GetNodeInfo(0, ip, mac, tpc, max_power);


  // Step 1: USE ROUTE_INFO TO GET THE ONE-HOP NEIGHBOR LIST
  //         [the list is stored in oneHopListHeader]
  struct oneHopList * temp;
  temp = oneHopListHeader;

  char dst_str[128];
  bzero(dst_str, 128);

  struct route_info	* rt_info;
  char *	v4_addr = const_cast<char*>(ip.c_str());
  char *	v6_addr = "3ffe::33"; // we are ignoring IPv6 addresses
  int		rt_info_result;
  char	gw_str[128];
  char	HNA[20];

  // Build the route_info request
  bzero(gw_str, 128);
  rt_info = (struct route_info *)malloc(sizeof(struct route_info));
  rt_info->version_flag = IS_V4;
  rt_info->dst_node_v4 = (struct in_addr *)malloc(sizeof(struct in_addr));
  rt_info->dst_node_v6 = (struct in6_addr *)malloc(sizeof(struct in6_addr));
  inet_pton(AF_INET, v4_addr, (rt_info->dst_node_v4));
  inet_pton(AF_INET6, v6_addr, (rt_info->dst_node_v6));
  sprintf(HNA, "./HNA");
  rt_info->whereHNA = HNA;

  // Make the request
  rt_info_result = req_route_info(rt_info);

#if defined(DEBUG)
  std::cout << std::endl;
  std::cout << "1-hop neighbor list" << std::endl;
  std::cout << "-------------------" << std::endl;

  for (temp = oneHopListHeader; temp != NULL; temp = temp->next){
    inet_ntop(AF_INET, temp->node_v4, dst_str, 128);
    std::cout << dst_str << std::endl;
  }

  std::cout << std::endl;
  std::cout << "Desired neighbor list" << std::endl;
  std::cout << "---------------------" << std::endl;

  std::vector<string>::iterator iter;
  for (iter = m_nbr_list.begin(); iter != m_nbr_list.end(); ++iter){
    std::cout << *iter << std::endl;
  }

  std::cout << std::endl;
  std::cout << "Exclusion list" << std::endl;
  std::cout << "--------------" << std::endl;

  for (iter = m_diff_list.begin(); iter != m_diff_list.end(); ++iter){
    std::cout << *iter << std::endl;
  }
#endif

  // We now have the 1-hop neighbor list
  // Step 2: look for a MATCH (examine all entries in the 1-hop list)
  int node_count = 0;
  int nbr_count = m_nbr_list.size();
  bool is_in_diff_list, is_in_nbr_list;
  result = 1; // increase the tx power level if nothing in the loop below matches
  for (temp = oneHopListHeader; temp != NULL; temp = temp->next){

    // Get the IP address of a 1-hop neighbor
    inet_ntop(AF_INET, temp->node_v4, dst_str, 128);
    ip = dst_str;

    // Search for the node in the two lists
    is_in_diff_list = binary_search(m_diff_list.begin(), m_diff_list.end(), ip);
    is_in_nbr_list = binary_search(m_nbr_list.begin(), m_nbr_list.end(), ip);

    // If at least one node IS in m_diff_list => reduce the tx power level
    if (is_in_diff_list){
      result = -1;
      break;
    }

    // If a node IS NOT in m_diff_list, but IS in m_nbr_list => ++node_count
    if ( !is_in_diff_list && is_in_nbr_list ){
      ++node_count;
      // If all nodes in m_nbr_list are in the 1-hop neighbor list => MATCH
      if (node_count == nbr_count){
        result = 0;
        break;
      }
    }

    // If a node IS NOT in m_diff_list and IS NOT in m_nbr_list => ignore
    if ( !is_in_diff_list && !is_in_nbr_list ){
      continue;
    }
  }

  // Inform tc_control of the result
  SetChangePower( result );

  return m_topol_match ? true : false;
}


////////
void TCTopologyController::GraphToTxPower()
{
  int OldTxPower = GetTxPower();

  // --- Open-loop strategy ---
  //       (table lookup)
  if (m_open_loop){
    // TBD: SET THE TX POWER LEVEL (table lookup)

  }

  // --- Closed-loop strategy ---
  //         (iterative)
  else {
    if (not m_busy){ // i.e., visiting the function for the first time
      // Lock the TC Controller
      Lock();

      // Set the topology match flag
      m_topol_match = false;

      /////////////////////////////
      // Set the upper and lower limits for tx_power level selection
      m_tx_upper_lim = 255;
      m_tx_lower_lim = 0;
    }

    // SET THE TX POWER LEVEL (binary search)
    if (m_change_power > 0){ // increase power level
      m_tx_lower_lim = GetTxPower(); // raise lower limit
      SetTxPower( (m_tx_upper_lim + m_tx_lower_lim)/2 ); // take the mean
    }

    if (m_change_power < 0){ // decrease power level
      m_tx_upper_lim = GetTxPower(); // reduce the upper limit
      SetTxPower( (m_tx_upper_lim + m_tx_lower_lim)/2 ); // take the mean
    }
  }

#if defined(DEBUG)
  std::cout << std::endl;
  std::cout << "Changing the power level from " << OldTxPower;
  std::cout << " to " << GetTxPower() << std::endl;
#endif
}


////////
void TCTopologyController::TCAlg(Graph& InG, Graph& OutG)
{
  typedef graph_traits<Graph>::edge_descriptor edge_t;
  typedef graph_traits<Graph>::vertex_descriptor vertex_t;
  vertex_t u, v;
  edge_t e;
  graph_traits<Graph>::vertex_iterator vi, vi_end, ui, ui_end;
  graph_traits<Graph>::edge_iterator ed, ed_end;

  // Edge list as a priority queue (ascending order of weights)
  typedef std::pair<double, edge_t> P;
  std::priority_queue<P, std::vector<P>, compare_first> pqE;

  // Add edges to the list
  for (tie(ed, ed_end) = edges(InG); ed != ed_end; ++ed){
    pqE.push( std::make_pair(InG[*ed].Weight, *ed) );
  }

  // We need to map from nodes in graph InG to graph OutG and vice-versa
  std::map<vertex_t, vertex_t> in2out, out2in;

  // Clear OutG, assign OutG = InG, then clear all edges in OutG (keep vertices)
  OutG.clear();
  OutG = InG;
  ui = vertices(InG).first;
  vi = vertices(OutG).first;
  for (unsigned int i = 0; i < num_vertices(OutG); ++i, ++vi, ++ui){
    in2out[*ui] = *vi;
    out2in[*vi] = *ui;
    clear_vertex(*vi, OutG);
  }

  // For each edge e(u,v) in the priority queue...
  // Note: the queue contains edges of InG, so we must map them into OutG first
  double w;
  bool inserted;
  while (!pqE.empty()){
    tie(w, e) = pqE.top();
    pqE.pop();
    u = in2out[source(e, OutG)];
    v = in2out[target(e, OutG)];

    // If there is no path from u to v in OutG, add (u,v) to OutG
    if (!exist_path(u, v, OutG)){
      tie(e, inserted) = add_edge(u, v, OutG);
      OutG[e].Weight = w;
    }
  }
}

//--------------------------------------------------------------------------------


///////////////////////////////
// Thread that checks if the desired topology graph (m_graph)
// matches the topology seen by the routing protocol
void *TCVerifyTopology( void *ptr )
{
  // Get the pointer to the TC Controller object
//  static TCTopologyController* tc_control = (TCTopologyController*) ptr;

  // Code here

  pthread_exit(0);
}

#endif // TC_TOPOLOGY_CONTROLLER_HPP
